/*
 * Created by SharpDevelop.
 * User: Tivadar Papai
 * Date: 8/27/2007
 * Time: 3:23 PM
 * 
 * To change this template use Tools | Options | Coding | Edit Standard Headers.
 */

using System;

namespace RoverStudio.Math
{
	/// <summary>
	/// Description of Rotation.
	/// </summary>
	public class Rotation
	{
        //public static readonly Rotation Identity = new Rotation(new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } });



        private double[,] matrix;

        public double[,] Matrix
        {
            get { return matrix; }
            set { SetMatrix(value); }
        }

        public Vector3D XAxis
        {
            get
            {
                return new Vector3D(matrix[0, 0], matrix[1, 0], matrix[2, 0]);
            }
        }

        public Vector3D YAxis
        {
            get
            {
                return new Vector3D(matrix[0, 1], matrix[1, 1], matrix[2, 1]);
            }
        }

        public Vector3D ZAxis
        {
            get
            {
                return new Vector3D(matrix[0, 2], matrix[1, 2], matrix[2, 2]);
            }
        }

        public Rotation()
        {
            matrix = new double[3, 3] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
        }

        public Rotation(double[,] matrix)
        {
            SetMatrix(matrix);
        }

        private void SetMatrix(double[,] matrix)
        {

            if ((matrix.GetLength(0) != 3) || (matrix.GetLength(1) != 3))
                throw new VectorGeometryException("The dimension of the given matrix should be 3x3.");
            for (int i = 0; i < 3; i++)
            {
                double lengthSquare = 0.0;
                for (int j = 0; j < 3; j++)
                {
                    //TODO: sor vagy oszlop
                    lengthSquare += matrix[i, j] * matrix[i, j];
                }
                if (lengthSquare < 0.99999)
                    throw new VectorGeometryException("Each orientation vector should be a normalized vector.");
            }
            this.matrix = matrix; // .Clone() ?
        }

        public static Rotation operator *(Rotation r1, Rotation r2)
        {
            Rotation res = new Rotation();
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                {
                    double tmp = 0;
                    for (int k = 0; k < 3; k++)
                        tmp += r1.matrix[i, k] * r2.matrix[k, j];
                    res.matrix[i, j] = tmp;
                }
            return res;
        }

        public Vector3D Apply(Vector3D vector)
        {
            Vector3D res = new Vector3D();
            res.X = vector.X * matrix[0, 0] + vector.Y * matrix[0, 1] + vector.Z * matrix[0, 2];
            res.Y = vector.X * matrix[1, 0] + vector.Y * matrix[1, 1] + vector.Z * matrix[1, 2];
            res.Z = vector.X * matrix[2, 0] + vector.Y * matrix[2, 1] + vector.Z * matrix[2, 2];
            return res;
        }
        public Rotation Inverse()
        {
            Rotation res = new Rotation();
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    res.matrix[i, j] = matrix[j, i];
            return res;
        }

	}
}
